from Ultralytics import YOLO
class DetectModel:
    def __init__(self,yolo_path):
        self.model = YOLO(yolo_path)

    def detect(self,frame):
        # 1. 这里需要调用yolo的预测功能，识别传入的frame
        # 2. 先获取一次模型里的预测类别总数
        # 3. 根据yolo11的results，获取识别到的ball的类名及其概率
        # 4. 如果某个类别的概率大于0.8，则存储在结果列表里
        # 5. 重复检测操作，直到当结果列表存满十个时，进行结算，返回当前视角下两个最多次检测到的目标
        # 5. 如果超过2s，没有新的概率超过0.8的结果存入结果聊表，则清空结果列表返回[(ball,probs),.......]
    def identifyObject(self,object,frame):
        # 1. 存储接收到的object目标，不断调用self.detect(frame)
        # 2. 持续比对detect的结果和object，如果存在对应的object，则返回(True,object)
        # 3. 如果没有超过2s，没有比对成功，则返回(False,object)
    def ManagerIdentify(self):
        # 该函数用来完成，根据identifyObject结果，进行调度
        # 如果接收到(True,object)，则调用measureDistance进行测距
        # 如果接收到(False,object)，则调用DogController，并传入字符串'turn'
    def DogTurn(self,angle):
        pass
        ## 这里保留不做修改，作为机器狗的底层控制
    def DogController(self,cmd):
        if cmd == 'turn':
            self.turn('180')
        
    def measureDistance(self,object):
        # 当self.identifyObject能够返回True和object时，执行测量距离函数
        # 继续调用YOLO.predict函数，筛选出object的probs，和标记框的四个像素坐标值
        # 当prob大于0.5时，调用相机的get_align_depth函数获取当前帧的深度图
        # 根据标记框的位置，读取标记框区域的平均深度值作为距离，返回(object,probs,distance)
        # 如果probs小于0.5时，则调用self.identifyObject再次确定目标,并准备接收self.identifyObject的返回值，继续完成定位成功后的距离测试
    def computeRelativePosition(self,obb,average_distance):
        # 根据平均距离和相机坐标系，计算目标距离狗的相对位置
        # 假定相机坐标系和机器狗坐标系重合
    def computeVelocity(self):
        # 根据相对位置，IMU数据，使用PID，计算出速度
        # 使用全向模型，机器狗支持x，y轴速度和z轴的旋转角
        # 计算成功后返回(vel_x,vel_y,yaw_z)
    def GlobalManager(self,object):
        # 根据传入的object，先定位目标是否在当前视角内
        # 如果在，则执行测距和相对位置计算，在完成速度计算，不断重复，直到目标距离只有0.1m
        # 如果不在，则旋转机器狗，指导寻找到目标，再执行第二步
#######
# 编写说明：留出获取相机图像的统一接口，机器狗的动作控制和速度下发的统一接口，我自行完成适配；编写代码要带有中文注释，注重代码架构优雅，可读性强，代码鲁棒性强。要多考虑边界情况。
#######

###
# 总体代码流程：
# 输入object给GolbalManager，Manager通过调用ManagerIdentify先检测是否在视角内，ManagerIdentify通过调用identifyObject开始检测，如果检测不到目标，则发送转向指令等待下一次检测
# 如果能检测到object，则调用measureDistance获取距离和位置
# measureDistance循环测量直到测距距离超过0.1m，或者测到超过2s继续测量目标，当检测到物体时，先计算相对位置
# 计算成功后，调用computeVelocity计算速度，计算成功后下发speed给DogController
# 如果测距成功，则调用measureDistance等待下一次的测量
# 如果第二次测距也成功，则调用GlobalManager继续下一个指令


# identifyObject获取prob，和obbs列表，循环调用detect，当满足一定条件(大于0.5，或者达到一定的检测次数)就获取第一个标记框位置返回给measureDistance


# measureDistance开始计算距离和z轴，调用get_align_depth读取深度图


# 计算成功后，返回给GlobalManager，GlobalManager计算速度，调用DogController下达控制指令，完成控制


# 如果没测距成功，会继续调用identifyObject，等待下一个指令

# 如果测量成功后，会先计算相对位置，然后进行位置和速度计算，当检测到距离不足0.1m时，会调用self.identifyObject(object)等待指令

# 如果接收到全局的指令后，会直接计算速度下发到DogController
###

